/*
	2007-2008 UMKC Robotics Team
	ATMega128 (CPU1) 
	
	Initializer File
	
	Updated: always updating
	
	Created By: Jared Bayne
	E-Mail:		JaredNBayne@gmail.com
	Created On:	February 1st, 2008

	Purpose:	This file will hold the wheel encoder functions.
	
	int5	rear driver wheel
	int4	rear passenger wheel


*/

#define Room_Sensor1	GET_BIT(PING).bit1
//#define Room_Sensor2	GET_BIT(PINE).bit4


void wheel_encoders_init(void);
void display_encoder_vals(int WhichOne);
void reset_encoders(void);
void test_90_degree_turn(int numtests, int speed, int counterval);
void test_1_revolution(void);
void wheel_encoder_tester(void);
void encoder_delay(int delay);
int average_all_encoders(void);

int average_all_encoders(void)
{

	return (Enc1+Enc2)/2;
}

void wheel_encoder_tester()
{
	while(1)
	{
		writeln("Enc1: ");
		lcddispint(Enc1);
		write(" Enc2: ");
		lcddispint(Enc2);
		
		lcdline2();
		
		write("Enc3: ");
		lcddispint(Enc3);
		write(" End4: ");
		lcddispint(Enc4);
		_delay_ms(500);
		
	}
}


void test_1_revolution(void)
{
	Enc1=Enc2=0;
	while(Enc1<34)
		go(Forward, 180, Forward,0);
	while(Enc2<34)
		go(Forward,0,Forward,180);
	
	stop();



}


void test_90_degree_turn(int numtests, int speed, int counterval)
{
	/*
		Not updated for 2008-2009
	
	
	*/

	for (int i=0;i<numtests;i++)
	{
		usart_writeln("Test ",1);
		usart_disp_int(i,1);
		reset_encoders();

		while(Enc1<counterval)
			go(Backward,speed,Forward,speed);
		
		stop();
		usart_writeln("Encoder 5: ",1);
		usart_disp_int(Enc1,1);
		usart_writeln("Encoder 7: ",1);
		usart_disp_int(Enc2,1);
		_delay_ms(1000);
	}
}



void reset_encoders(void)
{
	Enc1=Enc1Prev=0;
	Enc2=Enc2Prev=0;
	Enc3=Enc3Prev=0;
	Enc4=Enc4Prev=0;
}


void wheel_encoders_init(void)
{
	
	
	//EIMSK=0b00110000;		//put a 1 for every interrupt being used
	EIMSK = (1<<5) | (1<<4) | (1<<6) | (1<<7);
	
	//EICRB=0b00000101;		//generate int5 request onlow level
	EICRB = (1<<ISC50) | (1<<ISC40) | (1<<ISC60) | (1<<ISC70);
	
	sei(); // set enable interups (sEi)
}

void display_encoder_vals(int WhichOne)
{
	if (WhichOne==5)
	{

		if (Enc1!=Enc1Prev)
		{
			usart_writeln("Encoder 5: ",1);
			usart_disp_int(Enc1,1);
			Enc1Prev=Enc1;
		}
	
	}
	else if (WhichOne==4)
	{
		//USART1writeln("Encoder 5: ");
		//USART1DispInt(Enc1);
		if (Enc2!=Enc2Prev)
		{
			usart_writeln("Encoder 4: ",1);
			usart_disp_int(Enc2,1);
			Enc2Prev=Enc2;
		}
	
	
	
	}

}


ISR(INT7_vect) 
{ 
	//this function will be called when an interrupt on pin "int5" is triggered
	//triggering conditions is based on how the interrupts are initialized
	Enc4Prev=Enc4;
	Enc4++; 
} 

ISR(INT6_vect) 
{ 
	//this function will be called when an interrupt on pin "int5" is triggered
	//triggering conditions is based on how the interrupts are initialized
	Enc3Prev=Enc3;
	Enc3++; 
} 

ISR(INT5_vect) 
{ 
	//this function will be called when an interrupt on pin "int5" is triggered
	//triggering conditions is based on how the interrupts are initialized
	Enc1Prev=Enc1;
	Enc1++; 
} 

ISR(INT4_vect) 
{ 
	//this function will be called when an interrupt on pin "int4" is triggered
	//triggering conditions is based on how the interrupts are initialized
	Enc2Prev=Enc2;
	Enc2++; 
} 


ISR(BADISR_vect)
{
/*
	Function: 	Bad Interrupt
	
	Purpose:	This function is invoked whenever a bad
				interrupt number is called.  I.E. When 
				an interrupt happens that you haven't 
				planned for.
				
*/
	usart_writeln("Bad Interrupt Called",1);
    
}


void encoder_delay(int delay)
{
	reset_encoders();
	while(((Enc1+Enc2)/2) < delay)
	{
		_delay_ms(1);
	}
}

